#!/usr/bin/python
# -*- coding: UTF-8 -*-

import rospy
import tf
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
from std_msgs.msg import Float32

class OdomTf:
    def __init__(self):
        rospy.init_node("test_odom_tf")
        rospy.Subscriber("/odom",Odometry,callback=self.OdomCallback)
        rospy.Subscriber("/scan",LaserScan,callback=self.LaserScanCallback)


        odom_msg = Odometry()
        self.odom_combined_pub = rospy.Publisher("/odom_combined",Odometry,queue_size=10)
        self.scan_raw_pub = rospy.Publisher("/scan_raw",LaserScan,queue_size=10)

    def LaserScanCallback(self,msg):
        '''雷达数据回调函数
        '''
        scan_msg = msg
        scan_msg.header.frame_id = "laser"
        self.scan_raw_pub.publish(scan_msg)

    def OdomCallback(self,msg):
        '''里程计回调函数'''
        odom_msg = msg
        odom_msg.header.frame_id = "odom"
        odom_msg.child_frame_id = "base_link"

        br = tf.TransformBroadcaster()
        br.sendTransform(
                        (msg.pose.pose.position.x, 
                          msg.pose.pose.position.y, 
                          0),
                        # tf.transformations.quaternion_from_euler(0, 0, msg.theta),
                        (odom_msg.pose.pose.orientation.x,
                            odom_msg.pose.pose.orientation.y,
                            odom_msg.pose.pose.orientation.z,
                            odom_msg.pose.pose.orientation.w),
                        # rospy.Time.now(),
                        msg.header.stamp,
                        "base_link",
                        "odom")
        self.odom_combined_pub.publish(odom_msg)
    

if __name__ == "__main__":
    OdomTf()
    rospy.spin()